
/**********************************************************************\
*																																	  	 *
* PROGRAM: Arm Servo Driver    	      															  *
* VERSION: 1.2																										    *
* PURPOSE: This program uses a potentiometer to measure the height    *
*          of an arm, and runs the motor to a given height            *
* AUTHOR: Samuel Rosenstein                                           *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/

#ifndef ARMSERVO
#define ARMSERVO
#endif

#define leftB 2
#define rightB 1


void _allignHeight(int height) {
  int current;
  current = PotReading();

  height = max(height, MIN_HEIGHT_ARM);

  while (abs(height - current) > 10) {
    current = PotReading();
    if ((current - height) > 0) {
      motor[f] = 15;
    }
    if ((current - height) < 0) {
      motor[f] = -9;
    }
  }
  motor[f] = 0;
}

void allignHeight(int height) {
  int current;
  current = PotReading();
  if ((current - height) > 0) {
     _allignHeight(height - 50);
     wait1Msec(100);
  }
  _allignHeight(height);
}
